/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using Jitter2.Collision;
using Jitter2.Collision.Shapes;
using Jitter2.DataStructures;
using Jitter2.Dynamics.Constraints;
using Jitter2.LinearMath;
using Jitter2.UnmanagedMemory;

using SoftFloat;

namespace Jitter2.Dynamics
{
	
	[StructLayout(LayoutKind.Sequential)]
	public struct RigidBodyData
	{
	    public int _index;
	    public int _lockFlag;
	
	    public NVector3 Position;
	    public NVector3 Velocity;
	    public NVector3 AngularVelocity;
	
	    public NVector3 DeltaVelocity;
	    public NVector3 DeltaAngularVelocity;
	
	    public NQuaternion Orientation;
	    public JMatrix InverseInertiaWorld;
	
	    public sfloat InverseMass;
	    public bool IsActive;
	    public bool IsStatic;
		public bool IsTriggered;
		public CollisionLayer Layer;
	    
	
	    public readonly bool IsStaticOrInactive => !IsActive || IsStatic;
	}
	
	/// <summary>
	/// Represents the primary entity in the Jitter physics world.
	/// </summary>
	public sealed class RigidBody : IListIndex, IDebugDrawable
	{
	    internal JHandle<RigidBodyData> handle;
	
	    public readonly ulong RigidBodyId;
	
	    /// <summary>
	    /// Due to performance considerations, the data used to simulate this body (e.g., velocity or position)
	    /// is stored within a contiguous block of unmanaged memory. This refers to the raw memory location
	    /// and should seldom, if ever, be utilized outside of the engine. Instead, use the properties provided
	    /// by the <see cref="RigidBody"/> class itself.
	    /// </summary>
	    public ref RigidBodyData Data => ref handle.Data;
	
	    /// <summary>
	    /// Gets the handle to the rigid body data, see <see cref="Data"/>.
	    /// </summary>
	    public JHandle<RigidBodyData> Handle => handle;
	
	    internal readonly List<RigidBodyShape> shapes = new(1);
	
	    // There is only one way to create a body: world.CreateRigidBody. There, we add an island
	    // to the new body. This should never be null.
	    internal Island island = null!;
	
	    /// <summary>
	    /// Gets the collision island associated with this rigid body.
	    /// </summary>
	    public Island Island => island;
	
	    internal readonly List<RigidBody> connections = new(0);
	    internal readonly HashSet<Arbiter> contacts = new(0);
	    internal readonly HashSet<Constraint> constraints = new(0);
	
	    /// <summary>
	    /// Event triggered when a new arbiter is created, indicating that two bodies have begun colliding.
	    /// </summary>
	    /// <remarks>
	    /// This event provides an <see cref="Arbiter"/> object which contains details about the collision.
	    /// Use this event to handle logic that should occur at the start of a collision between two bodies.
	    /// </remarks>
	    public Action<Arbiter> BeginCollide;
	
	    /// <summary>
	    /// Event triggered when an arbiter is destroyed, indicating that two bodies have stopped colliding.
	    /// The reference to this arbiter becomes invalid after this call.
	    /// </summary>
	    /// <remarks>
	    /// This event provides an <see cref="Arbiter"/> object which contains details about the collision that has ended.
	    /// Use this event to handle logic that should occur when the collision between two bodies ends.
	    /// </remarks>
	    public Action<Arbiter> EndCollide;
	
	    internal void RaiseBeginCollide(Arbiter arbiter)
	    {
	        BeginCollide?.Invoke(arbiter);
	    }
	
	    internal void RaiseEndCollide(Arbiter arbiter)
	    {
	        EndCollide?.Invoke(arbiter);
	    }
	
	    /// <summary>
	    /// Contains all bodies this body is in contact with.
	    /// </summary>
	    public ReadOnlyList<RigidBody> Connections => new(connections);
	
	    /// <summary>
	    /// Contains all contacts in which this body is involved.
	    /// </summary>
	    public ReadOnlyHashSet<Arbiter> Contacts => new(contacts);
	
	    /// <summary>
	    /// Contains all constraints connected to this body.
	    /// </summary>
	    public ReadOnlyHashSet<Constraint> Constraints => new(constraints);
	
	    /// <summary>
	    /// Gets the list of shapes added to this rigid body.
	    /// </summary>
	    public ReadOnlyList<RigidBodyShape> Shapes => new(shapes);
	
	    internal int islandMarker;
	
	    internal sfloat sleepTime = (sfloat)0.0f;
	
	    internal sfloat inactiveThresholdLinearSq = (sfloat)0.1f;
	    internal sfloat inactiveThresholdAngularSq = (sfloat)0.1f;
	    internal sfloat deactivationTimeThreshold = (sfloat)1.0f;
	
	    internal sfloat linearDampingMultiplier = (sfloat)0.995f;
	    internal sfloat angularDampingMultiplier = (sfloat)0.995f;
	
	    internal JMatrix inverseInertia = JMatrix.Identity;
	    internal sfloat inverseMass = (sfloat)1.0f;
	
	    public sfloat Friction { get; set; } = (sfloat)0.2f;
	    public sfloat Restitution { get; set; } = (sfloat)0.0f;
	
	    private readonly int hashCode;
	
	    /// <summary>
	    /// Gets or sets the world assigned to this body.
	    /// </summary>
	    public World World { get; }
	
	    internal RigidBody(JHandle<RigidBodyData> handle, World world)
	    {
	        this.handle = handle;
	        World = world;
	
	        Data.Orientation = NQuaternion.Identity;
	        SetDefaultMassInertia();
	
	        RigidBodyId = World.RequestId();
	        uint h = (uint)RigidBodyId;
	
	        // The rigid body is used in hash-based data structures, provide a
	        // good hash - Thomas Wang, Jan 1997
	        h = h ^ 61 ^ (h >> 16);
	        h += h << 3;
	        h ^= h >> 4;
	        h *= 668265261U;
	        h ^= h >> 15;
	
	        hashCode = unchecked((int)h);
	
	        Data._lockFlag = 0;
	    }
	
	    /// <summary>
	    /// Gets or sets the deactivation time. If the magnitudes of both the angular and linear velocity of the rigid body
	    /// remain below the <see cref="DeactivationThreshold"/> for the specified time, the body is deactivated.
	    /// </summary>
	    public TimeSpan DeactivationTime
	    {
	        get => TimeSpan.FromSeconds((float)deactivationTimeThreshold);
	        set => deactivationTimeThreshold = value == TimeSpan.MaxValue ? sfloat.MaxValue : value == TimeSpan.MinValue ? sfloat.MinValue : value.TotalSeconds;
	    }
	
	    /// <summary>
	    /// Gets or sets the deactivation threshold. If the magnitudes of both the angular and linear velocity of the rigid body
	    /// remain below the specified values for the duration of <see cref="DeactivationTime"/>, the body is deactivated.
	    /// The threshold values are given in rad/s and length units/s, respectively.
	    /// </summary>
	    public (sfloat angular, sfloat linear) DeactivationThreshold
	    {
	        set
	        {
	            inactiveThresholdLinearSq = value.linear * value.linear;
	            inactiveThresholdAngularSq = value.angular * value.angular;
	        }
	    }
	
	    /// <summary>
	    /// Gets or sets the damping factors for linear and angular motion.
	    /// A damping factor of 0 means the body is not damped, while 1 brings
	    /// the body to a halt immediately. Damping is applied when calling
	    /// <see cref="World.Step(sfloat, bool)"/>. Jitter multiplies the respective
	    /// velocity each step by 1 minus the damping factor. Note that the values
	    /// are not scaled by time; a smaller time-step in
	    /// <see cref="World.Step(sfloat, bool)"/> results in increased damping.
	    /// </summary>
	    /// <remarks>
	    /// The damping factors should be within the range [0, 1].
	    /// </remarks>
	    public (sfloat linear, sfloat angular) Damping
	    {
	        get => ((sfloat)1.0f - linearDampingMultiplier, (sfloat)1.0f - angularDampingMultiplier);
	        set
	        {
	            if (value.linear < (sfloat)0.0f || value.linear > (sfloat)1.0f || value.angular < (sfloat)0.0f || value.angular > (sfloat)1.0f)
	            {
	                throw new ArgumentOutOfRangeException(nameof(value),
	                    "Damping multiplier has to be within [0, 1].");
	            }
	
	            linearDampingMultiplier = (sfloat)1.0f - value.linear;
	            angularDampingMultiplier = (sfloat)1.0f - value.angular;
	        }
	    }
	
	    public override int GetHashCode()
	    {
	        return hashCode;
	    }
	
	    private void SetDefaultMassInertia()
	    {
	        inverseInertia = JMatrix.Identity;
	        Data.InverseMass = (sfloat)1.0f;
	        UpdateWorldInertia();
	    }
	
	    public JMatrix InverseInertia => inverseInertia;
	
	    public NVector3 Position
	    {
	        get => handle.Data.Position;
	        set
	        {
	            handle.Data.Position = value;
	            Move();
	        }
	    }
	
	    public NQuaternion Orientation
	    {
	        get => Data.Orientation;
	        set
	        {
	            Data.Orientation = value;
	            Move();
	        }
	    }
	
	    private void Move()
	    {
	        UpdateWorldInertia();
	        foreach (RigidBodyShape shape in shapes)
	        {
	            World.UpdateShape(shape);
	        }
	
	        World.ActivateBodyNextStep(this);
	    }
	
	    public NVector3 Velocity
	    {
	        get => handle.Data.Velocity;
	        set => handle.Data.Velocity = value;
	    }
	
	    public NVector3 AngularVelocity
	    {
	        get => handle.Data.AngularVelocity;
	        set => handle.Data.AngularVelocity = value;
	    }
	
	    public bool AffectedByGravity { get; set; } = true;
	
	    /// <summary>
	    /// A managed pointer to custom user data. This is not utilized by the engine.
	    /// </summary>
	    public object? Tag { get; set; }
	
	    public bool EnableSpeculativeContacts { get; set; } = false;
	
	    private void UpdateWorldInertia()
	    {
	        if (Data.IsStatic)
	        {
	            Data.InverseInertiaWorld = JMatrix.Zero;
	            Data.InverseMass = (sfloat)0.0f;
	        }
	        else
	        {
	            var bodyOrientation = JMatrix.CreateFromQuaternion(Data.Orientation);
	            JMatrix.Multiply(bodyOrientation, inverseInertia, out Data.InverseInertiaWorld);
	            JMatrix.MultiplyTransposed(Data.InverseInertiaWorld, bodyOrientation, out Data.InverseInertiaWorld);
	            Data.InverseMass = inverseMass;
	        }
	    }
	
	    public bool IsStatic
	    {
	        set
	        {
	            if ((!Data.IsStatic && value) || (Data.IsStatic && !value))
	            {
	                Data.Velocity = NVector3.Zero;
	                Data.AngularVelocity = NVector3.Zero;
	            }
	
	            Data.IsStatic = value;
	            UpdateWorldInertia();
	
	            if (value) World.DeactivateBodyNextStep(this);
	            else World.ActivateBodyNextStep(this);
	        }
	        get => Data.IsStatic;
	    }
	
		public bool IsTriggered { get => Data.IsTriggered; set => Data.IsTriggered = value; }
		public CollisionLayer Layer { get => Data.Layer; set => Data.Layer = value; }
	    
	
	    /// <summary>
	    /// Indicates whether the rigid body is active or considered to be in a sleeping state.
	    /// Use <see cref="SetActivationState"/> to alter the activation state.
	    /// </summary>
	    public bool IsActive => Data.IsActive;
	
	    /// <summary>
	    /// Instructs Jitter to activate or deactivate the body at the commencement of
	    /// the next time step. The current state does not change immediately.
	    /// </summary>
	    public void SetActivationState(bool active)
	    {
	        if (active) World.ActivateBodyNextStep(this);
	        else World.DeactivateBodyNextStep(this);
	    }
	
	    private void AttachToShape(RigidBodyShape shape)
	    {
	        if (shape.RigidBody != null)
	        {
	            throw new ArgumentException("Shape has already been added to a body.", nameof(shape));
	        }
	
	        shape.RigidBody = this;
	        shape.UpdateWorldBoundingBox(sfloat.Zero);
	        World.DynamicTree.AddProxy(shape, IsActive);
	    }
	
	    /// <summary>
	    /// Adds several shapes to the rigid body at once. Mass properties are
	    /// recalculated only once, if requested.
	    /// </summary>
	    /// <param name="shapes">The Shapes to add.</param>
	    /// <param name="setMassInertia">If true, uses the mass properties of the Shapes to determine the
	    /// body's mass properties, assuming unit density for the Shapes. If false, the inertia and mass remain
	    /// unchanged.</param>
	    public void AddShape(IEnumerable<RigidBodyShape> shapes, bool setMassInertia = true)
	    {
	        foreach (RigidBodyShape shape in shapes)
	        {
	            AttachToShape(shape);
	        }
	
	        this.shapes.AddRange(shapes);
	        if (setMassInertia) SetMassInertia();
	    }
	
	    /// <summary>
	    /// Adds a shape to the body.
	    /// </summary>
	    /// <param name="shape">The shape to be added.</param>
	    /// <param name="setMassInertia">If true, utilizes the shape's mass properties to determine the body's
	    /// mass properties, assuming a unit density for the shape. If false, the inertia and mass remain unchanged.</param>
	    public void AddShape(RigidBodyShape shape, bool setMassInertia = true)
	    {
	        if (shape.IsRegistered)
	        {
	            throw new ArgumentException("Shape can not be added. Is the shape already registered?");
	        }
	
	        AttachToShape(shape);
	        shapes.Add(shape);
	        if (setMassInertia) SetMassInertia();
	    }
	
	    /// <summary>
	    /// Represents the force to be applied to the body during the next call to <see cref="World.Step(sfloat, bool)"/>.
	    /// This value is automatically reset to zero after the call.
	    /// </summary>
	    public NVector3 Force { get; set; }
	
	    /// <summary>
	    /// Represents the torque to be applied to the body during the next call to <see cref="World.Step(sfloat, bool)"/>.
	    /// This value is automatically reset to zero after the call.
	    /// </summary>
	    public NVector3 Torque { get; set; }
	
	    /// <summary>
	    /// Applies a force to the rigid body, thereby altering its velocity. This force is effective for a single frame only and is reset to zero during the next call to <see cref="World.Step(sfloat, bool)"/>.
	    /// </summary>
	    /// <param name="force">The force to be applied.</param>
	    public void AddForce(in NVector3 force)
	    {
	        Force += force;
	    }
	
	    /// <summary>
	    /// Applies a force to the rigid body, altering its velocity. This force is applied for a single frame only and is
	    /// reset to zero with the subsequent call to <see cref="World.Step(sfloat, bool)"/>.
	    /// </summary>
	    /// <param name="force">The force to be applied.</param>
	    /// <param name="position">The position where the force will be applied.</param>
	    [ReferenceFrame(ReferenceFrame.World)]
	    public void AddForce(in NVector3 force, in NVector3 position)
	    {
	        ref RigidBodyData data = ref Data;
	
	        if (data.IsStatic) return;
	
	        NVector3.Subtract(position, data.Position, out NVector3 torque);
	        NVector3.Cross(torque, force, out torque);
	
	        Force += force;
	        Torque += torque;
	    }
	
	    /// <summary>
	    /// Removes a specified shape from the rigid body.
	    /// </summary>
	    /// <remarks>This operation has a time complexity of O(n), where n is the number of shapes attached to the body.</remarks>
	    /// <param name="shape">The shape to remove from the rigid body.</param>
	    /// <param name="setMassInertia">Specifies whether to adjust the mass inertia properties of the rigid body after removing the shape. The default value is true.</param>
	    public void RemoveShape(RigidBodyShape shape, bool setMassInertia = true)
	    {
	        if (!shapes.Remove(shape))
	        {
	            throw new ArgumentException(
	                "Shape is not part of this body.");
	        }
	
	        foreach (var arbiter in contacts)
	        {
	            if (arbiter.Handle.Data.Key.Key1 == shape.ShapeId || arbiter.Handle.Data.Key.Key2 == shape.ShapeId)
	            {
	                // Removes the current element we are iterating over from Contacts, i.e. the HashSet
	                // we are iterating over is altered. This is allowed.
	                World.Remove(arbiter);
	            }
	        }
	
	        World.DynamicTree.RemoveProxy(shape);
	        shape.RigidBody = null!;
	
	        if (setMassInertia) SetMassInertia();
	    }
	
	    /// <summary>
	    /// Removes several shapes from the body.
	    /// </summary>
	    /// <remarks>This operation has a time complexity of O(n), where n is the number of shapes attached to the body.</remarks>
	    /// <param name="shapes">The shapes to remove from the rigid body.</param>
	    /// <param name="setMassInertia">Specifies whether to adjust the mass inertia properties of the rigid body after removal. The default value is true.</param>
	    public void RemoveShape(IEnumerable<RigidBodyShape> shapes, bool setMassInertia = true)
	    {
	        HashSet<ulong> sids = new HashSet<ulong>();
	
	        foreach (var shape in shapes)
	        {
	            if (shape.RigidBody != this)
	            {
	                throw new ArgumentException($"Shape {shape} is not attached to this body.", nameof(shapes));
	            }
	
	            sids.Add(shape.ShapeId);
	        }
	
	        foreach (var arbiter in contacts)
	        {
	            if (sids.Contains(arbiter.Handle.Data.Key.Key1) || sids.Contains(arbiter.Handle.Data.Key.Key2))
	            {
	                // Removes the current element we are iterating over from Contacts, i.e. the HashSet
	                // we are iterating over is altered. This is allowed.
	                World.Remove(arbiter);
	            }
	        }
	
	        for (int i = this.shapes.Count; i-- > 0;)
	        {
	            var shape = this.shapes[i];
	
	            if (sids.Contains(shape.ShapeId))
	            {
	                World.DynamicTree.RemoveProxy(shape);
	                shape.RigidBody = null!;
	                this.shapes.RemoveAt(i);
	            }
	        }
	
	        if (setMassInertia) SetMassInertia();
	
	        sids.Clear();
	    }
	
	    /// <summary>
	    /// Removes all shapes associated with the rigid body.
	    /// </summary>
	    /// <remarks>This operation has a time complexity of O(n), where n is the number of shapes attached to the body.</remarks>
	    /// <param name="setMassInertia">If set to false, the mass properties of the rigid body remain unchanged.</param>
	    //[Obsolete($"{nameof(ClearShapes)} is deprecated, please use {nameof(RemoveShape)} instead.")]
	    public void ClearShapes(bool setMassInertia = true)
	    {
	        RemoveShape(shapes, setMassInertia);
	    }
	
	    /// <summary>
	    /// Utilizes the mass properties of the shape to determine the mass properties of the rigid body.
	    /// </summary>
	    public void SetMassInertia()
	    {
	        if (shapes.Count == 0)
	        {
	            inverseInertia = JMatrix.Identity;
	            Data.InverseMass = (sfloat)1.0f;
	            return;
	        }
	
	        JMatrix inertia = JMatrix.Zero;
	        sfloat mass = (sfloat)0.0f;
	
	        for (int i = 0; i < shapes.Count; i++)
	        {
	            shapes[i].CalculateMassInertia(out var shapeInertia, out _, out var shapeMass);
	
	            inertia += shapeInertia;
	            mass += shapeMass;
	        }
	
	        if (!JMatrix.Inverse(inertia, out inverseInertia))
	        {
	            throw new InvalidOperationException("Inertia matrix is not invertible. This might happen if a shape has " +
	                                                "invalid mass properties. If you encounter this while calling " +
	                                                "RigidBody.AddShape, call AddShape with setMassInertia set to false.");
	        }
	
	        inverseMass = (sfloat)1.0f / mass;
	
	        UpdateWorldInertia();
	    }
	
	    /// <summary>
	    /// Sets a new mass value and scales the inertia according to the ratio of the old mass to the new mass.
	    /// </summary>
	    public void SetMassInertia(sfloat mass)
	    {
	        if (mass <= (sfloat)0.0f)
	        {
	            // we do not protect against NaN here, since it is the users responsibility
	            // to not feed NaNs to the engine.
	            throw new ArgumentException("Mass can not be zero or negative.", nameof(mass));
	        }
	
	        SetMassInertia();
	        inverseInertia = JMatrix.Multiply(inverseInertia, (sfloat)1.0f / (Data.InverseMass * mass));
	        inverseMass = (sfloat)1.0f / mass;
	        UpdateWorldInertia();
	    }
	
	    /// <summary>
	    /// Sets the new mass properties of this body by specifying both inertia and mass directly.
	    /// </summary>
	    /// <param name="setAsInverse">Set the inverse values.</param>
	    /// <exception cref="ArgumentException"></exception>
	    public void SetMassInertia(in JMatrix inertia, sfloat mass, bool setAsInverse = false)
	    {
	        if (setAsInverse)
	        {
	            if (sfloat.IsInfinity(mass) || mass < (sfloat)0.0f)
	            {
	                throw new ArgumentException("Inverse mass must be finite and not negative.", nameof(mass));
	            }
	
	            inverseInertia = inertia;
	            inverseMass = mass;
	        }
	        else
	        {
	            if (mass <= (sfloat)0.0f)
	            {
	                throw new ArgumentException("Mass can not be zero or negative.", nameof(mass));
	            }
	
	            if (!JMatrix.Inverse(inertia, out inverseInertia))
	            {
	                throw new ArgumentException("Inertia matrix is not invertible.", nameof(inertia));
	            }
	
	            inverseMass = (sfloat)1.0f / mass;
	        }
	
	        UpdateWorldInertia();
	    }
	
	    private static List<JTriangle>? debugTriangles;
	
	    /// <summary>
	    /// Generates a rough triangle approximation of the shapes of the body.
	    /// Since the generation is slow this should only be used for debugging
	    /// purposes.
	    /// </summary>
	    public void DebugDraw(IDebugDrawer drawer)
	    {
	        debugTriangles ??= new List<JTriangle>();
	
	        foreach (var shape in shapes)
	        {
	            ShapeHelper.MakeHull(shape, debugTriangles);
	
	            foreach (var tri in debugTriangles)
	            {
	                drawer.DrawTriangle(
	                    NVector3.Transform(tri.V0, Data.Orientation) + Data.Position,
	                    NVector3.Transform(tri.V1, Data.Orientation) + Data.Position,
	                    NVector3.Transform(tri.V2, Data.Orientation) + Data.Position);
	            }
	        }
	
	        debugTriangles.Clear();
	    }
	
	    /// <summary>
	    /// Gets the mass of the rigid body. To modify the mass, use
	    /// <see cref="RigidBody.SetMassInertia(sfloat)"/> or
	    /// <see cref="RigidBody.SetMassInertia(in JMatrix, sfloat, bool)"/>.
	    /// </summary>
	    public sfloat Mass => (sfloat)1.0f / inverseMass;
	
	    int IListIndex.ListIndex { get; set; } = -1;
	}
}
